import numpy as np
from scipy.stats import multivariate_normal

class HistogramFilter:
    def __init__(self, histogram, sigma=0.5):
        # self.belief = np.ones(len(histogram)) / len(histogram)
        self.belief = [0.6, 0.0, 0.2, 0.2]
        self.previous_prediction = None
        self.sigma = sigma
        self.histogram = histogram

    def updateBelief(self, prediction, observation):
        cov = np.array([[self.sigma, 0], [0, self.sigma]])
        rv = multivariate_normal(observation, cov)
        if self.previous_prediction is not None:
            for i, position in enumerate(prediction):
                diff = (observation[0] - position[0]) ** 2 + (observation[1] - position[1]) ** 2
                # self.belief[i] *= (1 / (diff ** 2 + 0.0001))
                self.belief[i] *= rv.pdf(position)
            #     print(self.histogram[i],' probability density is: ', rv.pdf(position))
            # print(self.belief)
        self.belief = self.belief / np.sum(self.belief)
        self.previous_prediction = prediction
